/*
 * Copyright 2025 The Authors
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 *@file    : frame.cc
 *@brief   : frame
 *@authors : zhanglei
 *@version : v1.0
 *@data    : 2025/9/14
 *
 */


#include "frame/frame.h"

namespace frame {

Frame::Frame(std::shared_ptr<FeatureDatabase> feature_database_ptr, const Eigen::Matrix3d& intrinsic_matrix, const Eigen::Matrix4d& extrinsic_matrix, const uint8_t& camera_id) : sliding_window_size_(10), feature_database_ptr_(feature_database_ptr), intrinsic_matrix_(intrinsic_matrix), extrinsic_matrix_(extrinsic_matrix), camera_id_(camera_id) {

}

void Frame::InsertSlidingWindowData(const uint64_t& frame_id, const State& state, const std::shared_ptr<ImuPreIntegration>& imu_preint_ptr, const std::shared_ptr<WheelPreIntegration>& wheel_preint_ptr, const Eigen::Vector2d& vel_world) {
    state_queue_.push_back(state);
    imu_preintegration_ptr_queue_.push_back(imu_preint_ptr);
    wheel_preintegration_ptr_queue_.push_back(wheel_preint_ptr);
    frame_ids_queue_.push_back(frame_id);
    velocity_queue_.push_back(vel_world);

    frame_id_ = frame_id;
}

void Frame::MarginalizationOldestFrame(const bool& is_using_visual_factor) {
    uint64_t oldest_frame_id = frame_id_-sliding_window_size_-1;

    if(state_queue_.size() > sliding_window_size_) {
        state_queue_.pop_front();
        imu_preintegration_ptr_queue_.pop_front();
        wheel_preintegration_ptr_queue_.pop_front();
        frame_ids_queue_.pop_front();
        velocity_queue_.pop_front();
        
        if(is_using_visual_factor) {
            feature_database_ptr_->EraseLandmarkFromFrameId(camera_id_, oldest_frame_id);
        }

        // LOG(INFO) << " oldest_frame_id : " << frame_id_ << " , " << oldest_frame_id << std::endl;
    }
}

uint8_t Frame::GetSlidingWindowSize() {
    return sliding_window_size_; 
}

uint64_t Frame::GetFrameId() {
    return frame_id_;
}

}
